#include "gnss.h"
#include <iostream>
#include <fstream>
#include <string>
#include <string.h>
#include <sstream>
#include <vector>
#include <iomanip>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <mutex>
#include <condition_variable>
#include <thread>

//函数声明
bool validateChecksum(const std::string &sentence);
std::vector<std::string> split(const std::string &s, char delimiter);
double nmeaToDecimal(const std::string &value, const std::string &direction);



Gnss::Gnss(const std::string& sensor_name_,const int& run_mode_, const std::string& dev_name_):SensorAbstraction(sensor_name_, run_mode_){
    dev_name = dev_name_;
    raw_data.reserve(512);
    p_gnss_data = std::make_unique<struct gnss_data>();
}

Gnss::~Gnss(){
    close(fd);
    std::cout<<sensor_name<<": ~Gnss"<<std::endl;
}

int Gnss::Init(){
    // 打开串口
    fd = open(dev_name.data(), O_RDWR | O_NOCTTY);
    if (fd < 0) {
        std::cerr <<sensor_name<< "Error opening serial port" << std::endl;
        return -1;
    }

    // 配置串口参数
    struct termios tty;
    const int BAUDRATE = B115200;
    memset(&tty, 0, sizeof tty);
    tcgetattr(fd, &tty);
    cfsetospeed(&tty, BAUDRATE);
    cfsetispeed(&tty, BAUDRATE);
    tty.c_cflag &= ~PARENB;    // 无奇偶校验
    tty.c_cflag &= ~CSTOPB;    // 1位停止位
    tty.c_cflag &= ~CSIZE;
    // 如果没有禁用回显，会导致输出$GNTXT,01,01,01,NMEA unknown msg*46信息
    tty.c_lflag &= ~ECHO;      // 禁用回显（关键修改）
    tty.c_lflag &= ~ICANON;    // 禁用规范模式
    tty.c_cflag |= CS8;        // 8位数据位
    tty.c_cc[VMIN] = 0;        // 非阻塞模式
    tty.c_cc[VTIME] = 1;       // 0.5秒超时
    tcsetattr(fd, TCSANOW, &tty);
    std::cout<<sensor_name<<": init successful"<<std::endl;
    return 0;
}
int Gnss::DataCollection() {
    int n = read(fd, buffer, sizeof(buffer));
    if (n > 0) {
        raw_data.append(buffer, n);
        // std::cout<<sensor_name<<": raw_data: "<<raw_data<<std::endl;
    }
    // std::cout<<sensor_name<<": data collect bytes: "<<n<<std::endl;
    
    // raw_data.clear();
    return 0;
}                           
int Gnss::DataProcessing() {
    // 按换行符分割数据
    size_t pos;
    while ((pos = raw_data.find("\n")) != std::string::npos) {
        std::string sentence = raw_data.substr(0, pos);
        // std::cout<<sensor_name<<": sentence: "<<sentence<<std::endl;
        raw_data.erase(0, pos+2); // 移除已处理数据
        // std::cout<<sensor_name<<": raw_data: "<<raw_data<<std::endl;
        // 校验基本格式
        if (sentence.empty() || sentence[0] != '$') continue;
        
        // 验证校验和
        if (!validateChecksum(sentence)) {
            std::cerr <<sensor_name<< ": Checksum failed: " << sentence << std::endl;
            continue;
        }

        // 分割字段
        std::vector<std::string> fields = split(sentence, ',');
        // for(size_t i = 0; i < fields.size(); ++i){
        //     std::cout<<sensor_name<<": fields["<<i<<"]: "<<fields[i]<<std::endl;
        // }
        // 处理GNRMC语句
        if (fields[0] == "$GNRMC" && fields.size() >= 12) {
            // 定位状态检查
            if(fields[2] == "V"){
                p_gnss_data->gnrmc_status = 0;
                std::cerr<<sensor_name<<": GNRMC status: V "<<std::endl;
                continue;
            }
            p_gnss_data->gnrmc_status = 1;
            p_gnss_data->cnt++;
            // 解析时间 hhmmss.ss ddmmyy
            std::string utc_time = fields[1];
            std::string utc_date = fields[9];
            if (utc_time.size() >= 6 && utc_date.size() == 6) {
                p_gnss_data->datetime = utc_date + "-" + utc_time.substr(0, 2) + ":" + utc_time.substr(2, 2) + ":" + utc_time.substr(4, 2);
            }
            
            // 解析纬度
            if (!fields[3].empty() && !fields[4].empty()) {
                p_gnss_data->latitude = nmeaToDecimal(fields[3], fields[4]);
            }
            
            // 解析经度
            if (!fields[5].empty() && !fields[6].empty()) {
                p_gnss_data->longitude = nmeaToDecimal(fields[5], fields[6]);
            }
            
        }
        
        // 处理GNGGA语句
        else if (fields[0] == "$GNGGA" && fields.size() >= 14) {
            // 检查定位质量指示
            if (fields[6].empty() || fields[6] == "0") {
                p_gnss_data->gngga_status = 0;
                std::cerr <<sensor_name<< ": GNGGA status: 0" << fields[6] << std::endl;
                continue;
            }
            p_gnss_data->gngga_status = 1;

            // 解析海拔
            if (!fields[9].empty()) {
                p_gnss_data->altitude = std::stod(fields[9]);
            }
            // 解析卫星数量
            if (!fields[7].empty()) {
                p_gnss_data->satellites_num = std::stod(fields[7]);  
            }
        }
    }
    return 0;
}                           
int Gnss::DataStorage() {
    // 拷贝p_gnss_data到p_gnss_data_shared
    {
        std::lock_guard<std::mutex> lock(gnss_data_mutex);  // 加互斥锁
        gnss_latest_data = *p_gnss_data;
        gnss_data_cv.notify_one();                          // 通知主线程，此时会释放互斥锁
    }
    
    return 0;
}                       


int Gnss::DataPrint(){
     std::cout <<sensor_name<< ": UTC Time: " << p_gnss_data->datetime;
     std::cout << " | Latitude: " << std::fixed << std::setprecision(6) << p_gnss_data->latitude << "°N";
     std::cout << " | Longitude: " << std::fixed << std::setprecision(6) << p_gnss_data->longitude << "°E";
     std::cout << " | Altitude: " << p_gnss_data->altitude << " m" ;
     std::cout << " | Satellites num: " << p_gnss_data->satellites_num << std::endl;
    return 0;
}



// 分割字符串函数
std::vector<std::string> split(const std::string &s, char delimiter) {
    std::vector<std::string> tokens;
    std::string token;
    std::istringstream tokenStream(s);
    while (getline(tokenStream, token, delimiter)) {
        tokens.push_back(token);
    }
    return tokens;
}

// 校验和验证
bool validateChecksum(const std::string &sentence) {
    size_t star_pos = sentence.find('*');
    if (star_pos == std::string::npos || star_pos + 3 > sentence.size()) return false;
    
    // 计算校验和
    uint8_t checksum = 0;
    for (size_t i = 1; i < star_pos; ++i) {
        checksum ^= sentence[i];
    }
    
    // 获取报文中的校验值
    std::string expected_str = sentence.substr(star_pos + 1, 2);
    unsigned int expected;
    std::istringstream iss(expected_str);
    iss >> std::hex >> expected;
    
    return checksum == expected;
}

// 转换度分格式为十进制
double nmeaToDecimal(const std::string &value, const std::string &direction) {
    size_t dot_pos = value.find('.');
    if (dot_pos == std::string::npos || dot_pos < 2) return 0.0;
    
    double degrees = std::stod(value.substr(0, dot_pos - 2));
    double minutes = std::stod(value.substr(dot_pos - 2));
    double decimal = degrees + minutes / 60.0;
    
    // 处理方向
    if (direction == "S" || direction == "W") decimal = -decimal;
    return decimal;
}

